#include "PatternOffset.h"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cmath>
#include <iostream>
#include <fstream>

int imageCutSizeCW = 1800;
int rotationWidth = 21;

// Considering distortion and magnification, the set floating range
int searchZong = 17;  // 
int searchHeng = 13;

std::vector<double> searchRatio{ 1.0 };
// Pro Pattern Mask
std::vector<int> zongDis1{ 496, 811, 936 };
std::vector<int> zongDis2{ 811, 496, 936 };
std::vector<int> hengDis1{ 483, 619, 619 };
std::vector<int> hengDis2{ 619, 483, 619 };
int sampleRatio = 3;

//// Evo Pattern Mask
//std::vector<int> zongDis1{ 464, 641, 842 };// (504, 286);
//std::vector<int> zongDis2{ 641, 464, 842 };// (504, 286);
//std::vector<int> hengDis1{ 464, 621, 0 };
//std::vector<int> hengDis2{ 464, 312, 621 };
//int sampleRatio = 2;

float PatternCalc::rotationCal2(cv::Mat& checkImg) {
	// Calculate image rotation angle through the accumulated brightness of lines
	cv::Point2f center(checkImg.cols / 2, checkImg.rows / 2);
	cv::Mat rot_mat, RotationImg, res1, res2;
	char outFile[512];
	double maxv, maxv2; cv::Point2i mpos, mpos2;
	int maxs = -100000000;
	int maxPos = 0;
	cv::Size dst_sz(checkImg.cols, checkImg.rows);
	for (int i = -6; i <= 6; i++) {
		float rotaionAngle = i * 0.2;
		rot_mat = cv::getRotationMatrix2D(center, rotaionAngle, 1.0);
		cv::warpAffine(checkImg, RotationImg, rot_mat, dst_sz);// , cv::INTER_LINEAR, cv::BORDER_REPLICATE);
		std::vector<int> sumCols, sumRows;
		int colsSum, rowsSum;
		for (int i = 0; i < RotationImg.rows; i++) {
			colsSum = 0;
			for (int j = 0; j < RotationImg.cols; j++) {
				colsSum += RotationImg.at<uchar>(i, j);
			}
			sumCols.push_back(colsSum);
		}
		for (int j = 0; j < RotationImg.cols; j++) {
			rowsSum = 0;
			for (int i = 0; i < RotationImg.rows; i++) {
				rowsSum += RotationImg.at<uchar>(i, j);
			}
			sumRows.push_back(rowsSum);
		}
		int maxCols = -1000;
		int sumxColsTemp, sumxRowsTemp;
		for (int searchI = 0; searchI <= sumCols.size() - rotationWidth; searchI++) {
			sumxColsTemp = sumCols[searchI] + sumCols[searchI + int((rotationWidth - 1) / 2)] + sumCols[searchI + rotationWidth - 1] - 1.5 * sumCols[searchI + int((rotationWidth - 1) / 2) - 3] - 1.5 * sumCols[searchI + int((rotationWidth - 1) / 2) + 3];
			if (sumxColsTemp > maxCols) maxCols = sumxColsTemp;
		}
		int maxRows = -1000;
		for (int searchI = 0; searchI <= sumRows.size() - rotationWidth; searchI++) {
			sumxRowsTemp = sumRows[searchI] + sumRows[searchI + int((rotationWidth - 1) / 2)] + sumRows[searchI + rotationWidth - 1] - 1.5 * sumRows[searchI + int((rotationWidth - 1) / 2) - 3] - 1.5 * sumRows[searchI + int((rotationWidth - 1) / 2) + 3];
			if (sumxRowsTemp > maxRows) maxRows = sumxRowsTemp;
		}
		if (maxCols + maxRows > maxs) {
			maxs = maxCols + maxRows;
			maxPos = i;
		}
	}
	return maxPos * 0.2;
}

float PatternCalc::rotationCal(cv::Mat& checkImg) {
	cv::Point2f center(checkImg.cols / 2, checkImg.rows / 2);
	cv::Mat rot_mat, RotationImg, res1, res2;
	char outFile[512];
	cv::Mat sum_Cols = cv::Mat::zeros(imageCutSizeCW, 13, CV_16SC1);
	cv::Mat sum_Rows = cv::Mat::zeros(13, imageCutSizeCW, CV_16SC1);

	for (int i = 0; i < imageCutSizeCW; i++) {
		sum_Cols.at<short>(i, 0) = 1;
		sum_Cols.at<short>(i, 6) = -1;
		sum_Cols.at<short>(i, 12) = 1;

		sum_Rows.at<short>(0, i) = 1;
		sum_Rows.at<short>(6, i) = -1;
		sum_Rows.at<short>(12, i) = 1;
	}
	double maxv, maxv2; cv::Point2i mpos, mpos2;
	double maxs = -1000;
	int maxPos = 0;
	cv::Size dst_sz(checkImg.cols, checkImg.rows);
	for (int i = -6; i <= 6; i++) {
		float rotaionAngle = i * 0.2;
		rot_mat = cv::getRotationMatrix2D(center, rotaionAngle, 1.0);
		cv::warpAffine(checkImg, RotationImg, rot_mat, dst_sz);// , cv::INTER_LINEAR, cv::BORDER_REPLICATE);
		cv::Point2f dd(-1, -1);
		cv::filter2D(RotationImg, res1, CV_32F, sum_Cols, dd, 0, cv::BORDER_CONSTANT);
		cv::filter2D(RotationImg, res2, CV_32F, sum_Rows, dd, 0, cv::BORDER_CONSTANT);// , dd, 0, cv::BORDER_TRANSPARENT);
		cv::minMaxLoc(res1,
			NULL, &maxv, NULL, &mpos);
		cv::minMaxLoc(res2,
			NULL, &maxv2, NULL, &mpos2);
		if ((maxv + maxv2) > maxs) {
			maxs = maxv + maxv2;
			maxPos = i;
		}
	}
	return maxPos * 0.2;
}

std::vector<int> PatternCalc::median_filter(std::vector<int> vec, int meanInt, bool norm, int scale) {
	// median filtering on the sum of image brightness accumulation
	int ranges = (scale - 1) / 2;

	int start, ends;
	std::vector<int> dst, temp;
	int newInt;
	for (int i = 0; i < vec.size(); ++i) {
		start = i - ranges < 0 ? 0 : i - ranges;
		ends = i + ranges >= vec.size() ? vec.size() - 1 : i + ranges;
		dst.resize(ends - start + 1);
		std::partial_sort_copy(vec.begin() + start, vec.begin() + ends, dst.begin(), dst.end());
		newInt = vec[i] * (meanInt * 1.0 / dst.at((int)((ends - start) / 2)));

		if (newInt > 0) {
			temp.push_back(newInt);
		}
		else {
			temp.push_back(meanInt);
		}
	}

	if (norm) {
		auto minPos = std::min_element(temp.begin(), temp.end());
		int minValue = *minPos;
		for (int i = 0; i < temp.size(); i++) {
			temp[i] -= minValue;
		}
	}
	return temp;
}

int PatternCalc::findPeaks(std::vector<int> intsVector, std::vector<float>& maxpos, std::vector<float>& maxvalue) {
	// find peaks position of feature curves 
	int sz = intsVector.size();
	if (sz == 0) return -1;
	double xvmean = 0.0, xvstd_1 = 0.0;
	for (int i = 0; i < sz; i++) {
		xvmean += intsVector[i];
	}
	xvmean /= sz;
	for (int i = 0; i < sz; i++) {
		xvstd_1 += ((intsVector[i] - xvmean)*(intsVector[i] - xvmean));
	}
	xvstd_1 =  std::sqrt(xvstd_1/sz);
	double threshold = xvmean + 2.999*xvstd_1;
	std::vector<int> choosePos, current_tcPos;
	std::vector<std::vector<int>> tcPoss;
	int idx, Dis = 15;
	for (int i = Dis; i < sz - Dis; i++) {
		if (intsVector[i] > threshold) choosePos.push_back(i);
	}
	if (choosePos.size() < 5) {
		return -1;
	}
	int first_Id = choosePos[0];
	for (int i = 1; i < choosePos.size(); i++) {
		idx = choosePos[i];
		if (idx - first_Id <= 50)
		{
			current_tcPos.push_back(idx);
		}
		else
		{
			tcPoss.push_back(current_tcPos);
			current_tcPos.clear();
			first_Id = idx;

		}
	}
	if (current_tcPos.size() > 0) tcPoss.push_back(current_tcPos);
	std::vector<double> xmaxl, peaks;
	double xmax, valueMax;
	idx = 0;
	for (int i = 0; i < tcPoss.size(); i++) {
		xmax = 0, valueMax = -10000000000.1;
		for (int j = 0; j < tcPoss[i].size(); j++)
		{
			idx = tcPoss[i][j];
			if (intsVector[idx] > valueMax) {
				valueMax = intsVector[idx];
				xmax = idx;
			}
		}
		if (xmax > 0) {
			maxpos.push_back(xmax);
			maxvalue.push_back(valueMax);
		}
	}
	return 0;
}

int PatternCalc::degreeOfConfidence(std::vector<int> zongVector, std::vector<int> hengVector, cv::Point2i& lineNum) {
	std::vector<float> zongMaxPos, zongMaxValue;
	std::vector<float> hengMaxPos, hengMaxValue;
	findPeaks(zongVector, zongMaxPos, zongMaxValue);
	findPeaks(hengVector, hengMaxPos, hengMaxValue);
	lineNum.x = zongMaxPos.size();
	lineNum.y = hengMaxPos.size();
	return 0;
}


void PatternCalc::calOffsetPattern(cv::Mat& img, cv::Point2f& offsetRes, bool& useOffsetFlag) {
	cv::Mat centerImg = img(cv::Rect(img.cols / 2 - int(imageCutSizeCW / 2), img.rows / 2 - int(imageCutSizeCW / 2), imageCutSizeCW, imageCutSizeCW));

	// Calculate rotation angle and correct it
	float rotationAngle = rotationCal2(centerImg);
	cv::Point2f center(img.cols / 2, img.rows / 2);
	cv::Mat rot_mat = cv::getRotationMatrix2D(center, rotationAngle, 1.0);
	cv::Size dst_sz(img.cols, img.rows);
	cv::warpAffine(img, img, rot_mat, dst_sz);

	// Calculate the horizontal and vertical cumulative sum of images
	std::vector<int> zong, heng;
	int sums, totalSum1, totalSum2;
	totalSum1 = 0;
	totalSum2 = 0;
	for (int i = 0; i < img.cols; i++) {
		sums = 0;
		for (int j = 0; j < img.rows; j++) {
			sums += img.at<uchar>(j, i);
		}
		totalSum1 += sums;
		zong.push_back(sums);
	}
	totalSum1 /= img.cols;

	for (int j = 0; j < img.rows; j++) {
		sums = 0;
		for (int i = 0; i < img.cols; i++) {
			sums += img.at<uchar>(j, i);
		}
		totalSum2 += sums;
		heng.push_back(sums);
	}
	totalSum2 /= img.rows;

	// Perform median filtering on the brightness accumulation and curve
	std::vector<int> zongNew = median_filter(zong, totalSum1);
	std::vector<int> hengNew = median_filter(heng, totalSum2);

	// W filter of track-Line
	int zongTemp;
	std::vector<int> zongVector, zongVectorID;
	//for (int searchI = 0; searchI <= zongNew.size() - rotationWidth; searchI++) {
	//	zongTemp = zongNew[searchI] + zongNew[searchI + int((rotationWidth - 1) / 2)] + zongNew[searchI + rotationWidth - 1] - 1.5 * zongNew[searchI + int((rotationWidth - 1) / 2) - 3] - 1.5 * zongNew[searchI + int((rotationWidth - 1) / 2) + 3];
	//	zongVector.push_back(zongTemp);
	//	zongVectorID.push_back(searchI + int((rotationWidth - 1) / 2));
	//}
	for (int searchI = 0; searchI <= zongNew.size() - rotationWidth; searchI++) {
		zongTemp = -zongNew[searchI + int((rotationWidth - 1) / 2) - sampleRatio] - zongNew[searchI + int((rotationWidth - 1) / 2) + sampleRatio]
			- zongNew[searchI + int((rotationWidth - 1) / 2) - (sampleRatio - 1)] - zongNew[searchI + int((rotationWidth - 1) / 2) + (sampleRatio - 1)];
		zongVector.push_back(zongTemp);
		zongVectorID.push_back(searchI + int((rotationWidth - 1) / 2));
	}

	int hengTemp;
	std::vector<int> hengVector, hengVectorID;
	//for (int searchI = 0; searchI <= hengNew.size() - rotationWidth; searchI++) {
	//	hengTemp = hengNew[searchI] + hengNew[searchI + int((rotationWidth - 1) / 2)] + hengNew[searchI + rotationWidth - 1] - 1.5 * hengNew[searchI + int((rotationWidth - 1) / 2) - 3] - 1.5 * hengNew[searchI + int((rotationWidth - 1) / 2) + 3];
	//	hengVector.push_back(hengTemp);
	//	hengVectorID.push_back(searchI + int((rotationWidth - 1) / 2));
	//}
	for (int searchI = 0; searchI <= hengNew.size() - rotationWidth; searchI++) {
		hengTemp = -hengNew[searchI + int((rotationWidth - 1) / 2) - sampleRatio] - hengNew[searchI + int((rotationWidth - 1) / 2) + sampleRatio]
			- hengNew[searchI + int((rotationWidth - 1) / 2) - (sampleRatio - 1)] - hengNew[searchI + int((rotationWidth - 1) / 2) + (sampleRatio - 1)];
		hengVector.push_back(hengTemp);
		hengVectorID.push_back(searchI + int((rotationWidth - 1) / 2));
	}

	// serach offset
	int offsetY = 0, offsetX = 0;;
	for (int searchNum = 0; searchNum < searchRatio.size(); searchNum++) {
		int zongMin1 = -1000000000, zongMin2 = -1000000000;//000000000
		int zongMinPos1 = 0, zongMinPos2 = 0;

		int zongValue1, zongValue2;
		int minZongTemp, minHengTemp;
		for (int i = 0; i < zongVector.size(); i++) {
			if ((i - zongDis1[0] - zongDis1[1] - searchZong) > 0) {
				zongValue1 = zongVector[i];
				minZongTemp = -100000000;
				for (int zongId = i - zongDis1[1] - searchZong; zongId <= i - zongDis1[1] + searchZong; zongId++) {
					if (zongVector[zongId] > minZongTemp) minZongTemp = zongVector[zongId];
				}
				zongValue1 += minZongTemp;
				minZongTemp = -100000000;
				for (int zongId = i - zongDis1[0] - zongDis1[1] - searchZong; zongId <= i - zongDis1[0] - zongDis1[1] + searchZong; zongId++) {
					if (zongVector[zongId] > minZongTemp) minZongTemp = zongVector[zongId];
				}
				zongValue1 += minZongTemp;
				if (zongValue1 > zongMin1) {
					zongMin1 = zongValue1;
					zongMinPos1 = zongVectorID[i];
				}
			}

			if ((i + zongDis2[0] + zongDis2[1] + searchZong) < zongVector.size()) {
				zongValue2 = zongVector[i];
				minZongTemp = -100000000;
				for (int zongId = i + zongDis2[0] - searchZong; zongId <= i + zongDis2[0] + searchZong; zongId++) {
					if (zongVector[zongId] > minZongTemp) minZongTemp = zongVector[zongId];
				}
				zongValue2 += minZongTemp;
				minZongTemp = -100000000;
				for (int zongId = i + zongDis2[0] + zongDis2[1] - searchZong; zongId <= i + zongDis2[0] + zongDis2[1] + searchZong; zongId++) {
					if (zongVector[zongId] > minZongTemp) minZongTemp = zongVector[zongId];
				}
				zongValue2 += minZongTemp;
				if (zongValue2 > zongMin2) {
					zongMin2 = zongValue2;
					zongMinPos2 = zongVectorID[i];
				}
			}
		}
		int offsetType;
		if (zongMin2 < zongMin1) {
			offsetX = zongMinPos1 + zongDis2[1] / 2 - (img.cols / 2);
			offsetType = -1;
		}
		else {
			offsetX = zongMinPos2 - zongDis2[2] / 2 - (img.cols / 2);
			offsetType = 1;
		}
		int offsetXTemp = zongMinPos2 - zongDis2[2] / 2 - (img.cols / 2);
		if (std::abs(offsetX) < 100 && std::abs(offsetXTemp - offsetX) < 20) {
			offsetX = offsetXTemp;
		}

		int hengMin1 = -1000000000, hengMin2 = -1000000000;
		int hengMinPos1, hengMinPos2 = 0;
		int hengValue1, hengValue2;
		for (int i = 0; i < hengVector.size(); i++) {
			if (i - hengDis1[0] - hengDis1[1] - searchHeng > 0) {
				hengValue1 = hengVector[i];
				minHengTemp = -100000000;
				for (int hengId = i - hengDis1[1] - searchHeng; hengId <= i - hengDis1[1] + searchHeng; hengId++) {
					if (hengVector[hengId] > minHengTemp) minHengTemp = hengVector[hengId];
				}
				hengValue1 += minHengTemp;
				minHengTemp = -100000000;
				for (int hengId = i - hengDis1[0] - hengDis1[1] - searchHeng; hengId <= i - hengDis1[0] - hengDis1[1] + searchHeng; hengId++) {
					if (hengVector[hengId] > minHengTemp) minHengTemp = hengVector[hengId];
				}
				hengValue1 += minHengTemp;
				if (hengValue1 > hengMin1) {
					hengMin1 = hengValue1;
					hengMinPos1 = hengVectorID[i];
				}
			}
			if ((i + hengDis2[1] + hengDis2[0] + searchHeng) < hengVector.size()) {
				hengValue2 = hengVector[i];
				minHengTemp = -100000000;
				for (int hengId = i + hengDis2[0] - searchHeng; hengId <= i + hengDis2[0] + searchHeng; hengId++) {
					if (hengVector[hengId] > minHengTemp) minHengTemp = hengVector[hengId];
				}
				hengValue2 += minHengTemp;
				minHengTemp = -100000000;
				for (int hengId = i + hengDis2[0] + hengDis2[1] - searchHeng; hengId <= i + hengDis2[0] + hengDis2[1] + searchHeng; hengId++) {
					if (hengVector[hengId] > minHengTemp) minHengTemp = hengVector[hengId];
				}
				hengValue2 += minHengTemp;
				if (hengValue2 > hengMin2) {
					hengMin2 = hengValue2;
					hengMinPos2 = hengVectorID[i];
				}
			}
		}
		if (hengMin2 < hengMin1) {
			offsetY = hengMinPos1 - hengDis1[1] / 2 - (img.rows / 2);
			offsetType = -1;
		}
		else {
			offsetY = hengMinPos2 + hengDis2[2] / 2 - (img.rows / 2);
			offsetType = 1;
		}
		int offsetYTemp = hengMinPos1 - hengDis1[1] / 2 - (img.rows / 2);
		if (std::abs(offsetY) < 100 && std::abs(offsetYTemp - offsetY) < 20) {
			offsetY = offsetYTemp;
		}
	}

	if (offsetY <= -img.rows / 4) {
		offsetY = (offsetY + img.rows / 2);
	}
	else if (offsetY > img.rows / 4) {
		offsetY = (offsetY - img.rows / 2);
	}
	if (offsetX <= -img.cols / 2) {
		offsetX = (offsetX + img.cols);
	}
	else if (offsetX > img.cols / 2) {
		offsetX = (offsetX - img.cols);
	}

	//offsetRes.x = -1 * offsetX;
	//offsetRes.y = -1 * offsetY;
	offsetRes.x = offsetY;
	offsetRes.y = -1 * offsetX;
	cv::Point2i linenum;
	degreeOfConfidence(zongVector, hengVector, linenum);
	if ((linenum.x >= 5 && linenum.x <= 6 && linenum.y == 4) || (linenum.x == 6 && linenum.y >= 3 && linenum.y <= 4)) useOffsetFlag = true;
	//std::cout << "linenum: " << linenum.x << "," << linenum.y << std::endl;

}
